<!-- saved from url=(0014)about:internet -->
<pre class="code">
<span class="srcline"><span class="lineno"><a href="1,1" id="srcline1"> 1</a></span><span class="line"><span class="keyword">function</span> [ <span class="var type1" id="S2T9U3">h</span> <span class="var type1" id="S3T9U4">v</span> <span class="var type1" id="S4T9U5">a</span> <span class="var type1" id="S5T9U6">a_a</span>] = nav_h_ekf(<span class="var type1" id="S6T2U9">Q</span>, <span class="var type1" id="S7T13U10">H</span>, <span class="var type1" id="S8T15U11">R</span> ,<span class="var type1" id="S9T2U12">F</span>, <span class="var type1" id="S10T2U13">I</span> ,<span class="var type1" id="S11T27U14">Z</span>,<span class="var type1" id="S12T12U15">up</span> )</span></span>
<span class="srcline"><span class="lineno"><a href="1,2" id="srcline2"> 2</a></span><span class="line"><span class="comment">%作者 ytf</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,3" id="srcline3"> 3</a></span><span class="line"><span class="comment">%日期 2017年4月27日</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,4" id="srcline4"> 4</a></span><span class="line"><span class="comment">%状态转移协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,5" id="srcline5"> 5</a></span><span class="line"><span class="comment">%H 预测矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,6" id="srcline6"> 6</a></span><span class="line"><span class="comment">%R 观测噪声方差</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,7" id="srcline7"> 7</a></span><span class="line"><span class="comment">%F 状态转移矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,8" id="srcline8"> 8</a></span><span class="line"><span class="comment">%I</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,9" id="srcline9"> 9</a></span><span class="line"><span class="comment">%Z 测量矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,10" id="srcline10">10</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,11" id="srcline11">11</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S13T3U17">X</span> ;<span class="comment">% 状态变量</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,12" id="srcline12">12</a></span><span class="line">          <span class="keyword">if</span>(<span class="mxinfo" id="T1:U13"><span class="mxinfo" id="T1:U14">isempty(<span class="var type0" id="S13T0U23">X</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,13" id="srcline13">13</a></span><span class="line">           <span class="mxinfo" id="T3:U15"><span class="var type1" id="S13T3U26">X</span>=<span class="mxinfo" id="T3:U17">single(<span class="mxinfo" id="T34:U18">[0 0 0 0]'</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,14" id="srcline14">14</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,15" id="srcline15">15</a></span><span class="line">          </span></span>
<span class="srcline"><span class="lineno"><a href="1,16" id="srcline16">16</a></span><span class="line"> <span class="keyword">persistent</span> <span class="var type1" id="S16T2U37">P</span> ;<span class="comment">% 状态协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,17" id="srcline17">17</a></span><span class="line">            <span class="keyword">if</span>(<span class="mxinfo" id="T1:U20"><span class="mxinfo" id="T1:U21">isempty(<span class="var type0" id="S16T0U43">P</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,18" id="srcline18">18</a></span><span class="line">            <span class="mxinfo" id="T2:U22"><span class="var type1" id="S16T2U46">P</span>=<span class="mxinfo" id="T2:U24">single(<span class="mxinfo" id="T35:U25">[100 0 0 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,19" id="srcline19">19</a></span><span class="line"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T2:U24"><span class="mxinfo" id="T35:U25">                     0 100 0 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,20" id="srcline20">20</a></span><span class="line"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T2:U24"><span class="mxinfo" id="T35:U25">                     0 0 100 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,21" id="srcline21">21</a></span><span class="line"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T2:U24"><span class="mxinfo" id="T35:U25">                     0 0 0  1000]</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,22" id="srcline22">22</a></span><span class="line">            <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,23" id="srcline23">23</a></span><span class="line">       <span class="mxinfo" id="T3:U26"><span class="var type1" id="S13T3U72">X</span>=<span class="mxinfo" id="T3:U28"><span class="var type1" id="S9T2U74">F</span>*<span class="var type1" id="S13T3U75">X</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,24" id="srcline24">24</a></span><span class="line">       <span class="mxinfo" id="T2:U31"><span class="var type1" id="S16T2U78">P</span>=<span class="mxinfo" id="T2:U33"><span class="mxinfo" id="T2:U34"><span class="mxinfo" id="T2:U35"><span class="var type1" id="S9T2U82">F</span>*<span class="var type1" id="S16T2U83">P</span></span>*<span class="mxinfo" id="T2:U38"><span class="var type1" id="S9T2U85">F</span>'</span></span>+<span class="var type1" id="S6T2U86">Q</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,25" id="srcline25">25</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T1:U41"><span class="var type1" id="S12T12U90">up</span>==<span class="mxinfo" id="T6:U43">1</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,26" id="srcline26">26</a></span><span class="line">       <span class="mxinfo" id="T14:U44"><span class="var type1" id="S17T14U94">K</span>=<span class="mxinfo" id="T14:U46"><span class="mxinfo" id="T14:U47"><span class="var type1" id="S16T2U97">P</span>*<span class="mxinfo" id="T14:U49"><span class="var type1" id="S7T13U99">H</span>'</span></span>/<span class="mxinfo" id="T15:U51">pinv(<span class="mxinfo" id="T15:U52"><span class="mxinfo" id="T15:U53"><span class="mxinfo" id="T13:U54"><span class="var type1" id="S7T13U105">H</span>*<span class="var type1" id="S16T2U106">P</span></span>*<span class="mxinfo" id="T14:U57"><span class="var type1" id="S7T13U108">H</span>'</span></span> +<span class="var type1" id="S8T15U109">R</span></span>)</span></span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,27" id="srcline27">27</a></span><span class="line">       <span class="mxinfo" id="T3:U60"><span class="var type1" id="S13T3U112">X</span>=<span class="mxinfo" id="T3:U62"><span class="var type1" id="S13T3U114">X</span>+<span class="mxinfo" id="T3:U64"><span class="var type1" id="S17T14U116">K</span>*(<span class="mxinfo" id="T27:U66"><span class="var type1" id="S11T27U119">Z</span>-<span class="mxinfo" id="T27:U68"><span class="var type1" id="S7T13U121">H</span>*<span class="var type1" id="S13T3U122">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,28" id="srcline28">28</a></span><span class="line">       <span class="mxinfo" id="T2:U71"><span class="var type1" id="S16T2U125">P</span>=<span class="mxinfo" id="T2:U73">(<span class="mxinfo" id="T2:U74">single(<span class="var type1" id="S10T2U131">I</span>)-<span class="mxinfo" id="T2:U76"><span class="var type1" id="S17T14U133">K</span>*<span class="var type1" id="S7T13U134">H</span></span></span>)*<span class="var type1" id="S16T2U135">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,29" id="srcline29">29</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,30" id="srcline30">30</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,31" id="srcline31">31</a></span><span class="line">       <span class="keyword">if</span> <span class="mxinfo" id="T1:U80"><span class="var type1" id="S12T12U139">up</span>==<span class="mxinfo" id="T6:U82">0</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,32" id="srcline32">32</a></span><span class="line">       <span class="mxinfo" id="T33:U83"><span class="var type1" id="S19T33U143">H_H</span>=<span class="mxinfo" id="T33:U85"><span class="var type1" id="S7T13U145">H</span>(<span class="mxinfo" id="T6:U87">2</span>,:)</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,33" id="srcline33">33</a></span><span class="line">       <span class="mxinfo" id="T3:U88"><span class="var type1" id="S17T3U150">K</span>=<span class="mxinfo" id="T3:U90"><span class="mxinfo" id="T3:U91"><span class="var type1" id="S16T2U153">P</span>*<span class="mxinfo" id="T3:U93"><span class="var type1" id="S19T33U155">H_H</span>'</span></span>/<span class="mxinfo" id="T9:U95">pinv(<span class="mxinfo" id="T9:U96"><span class="mxinfo" id="T9:U97"><span class="mxinfo" id="T33:U98"><span class="var type1" id="S19T33U161">H_H</span>*<span class="var type1" id="S16T2U162">P</span></span>*<span class="mxinfo" id="T3:U101"><span class="var type1" id="S19T33U164">H_H</span>'</span></span> +<span class="mxinfo" id="T9:U103"><span class="var type1" id="S8T15U166">R</span>(<span class="mxinfo" id="T6:U105">2</span>,<span class="mxinfo" id="T6:U106">2</span>)</span></span>)</span></span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,34" id="srcline34">34</a></span><span class="line">       <span class="mxinfo" id="T3:U107"><span class="var type1" id="S13T3U171">X</span>=<span class="mxinfo" id="T3:U109"><span class="var type1" id="S13T3U173">X</span>+<span class="mxinfo" id="T3:U111"><span class="var type1" id="S17T3U175">K</span>*(<span class="mxinfo" id="T9:U113"><span class="mxinfo" id="T9:U114"><span class="var type1" id="S11T27U179">Z</span>(<span class="mxinfo" id="T6:U116">2</span>)</span>-<span class="mxinfo" id="T9:U117"><span class="var type1" id="S19T33U182">H_H</span>*<span class="var type1" id="S13T3U183">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,35" id="srcline35">35</a></span><span class="line">       <span class="mxinfo" id="T2:U120"><span class="var type1" id="S16T2U186">P</span>=<span class="mxinfo" id="T2:U122">(<span class="mxinfo" id="T2:U123">single(<span class="var type1" id="S10T2U192">I</span>)-<span class="mxinfo" id="T2:U125"><span class="var type1" id="S17T3U194">K</span>*<span class="var type1" id="S19T33U195">H_H</span></span></span>)*<span class="var type1" id="S16T2U196">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,36" id="srcline36">36</a></span><span class="line">       <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,37" id="srcline37">37</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,38" id="srcline38">38</a></span><span class="line">       <span class="mxinfo" id="T9:U129"><span class="var type1" id="S2T9U199">h</span>=<span class="mxinfo" id="T9:U131"><span class="var type1" id="S13T3U201">X</span>(<span class="mxinfo" id="T6:U133">1</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,39" id="srcline39">39</a></span><span class="line">       <span class="mxinfo" id="T9:U134"><span class="var type1" id="S3T9U205">v</span>=<span class="mxinfo" id="T9:U136"><span class="var type1" id="S13T3U207">X</span>(<span class="mxinfo" id="T6:U138">2</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,40" id="srcline40">40</a></span><span class="line">       <span class="mxinfo" id="T9:U139"><span class="var type1" id="S4T9U211">a</span>=<span class="mxinfo" id="T9:U141"><span class="var type1" id="S13T3U213">X</span>(<span class="mxinfo" id="T6:U143">3</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,41" id="srcline41">41</a></span><span class="line">       <span class="mxinfo" id="T9:U144"><span class="var type1" id="S5T9U217">a_a</span>=<span class="mxinfo" id="T9:U146"><span class="var type1" id="S13T3U219">X</span>(<span class="mxinfo" id="T6:U148">4</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,42" id="srcline42">42</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,43" id="srcline43">43</a></span><span class="line">       </span></span>
<span class="srcline"><span class="lineno"><a href="1,44" id="srcline44">44</a></span><span class="line">       </span></span>
</pre>
